
int openclaw(){
	set_servo_position(2,800);}
int closeclaw(){
	set_servo_position(2,0);}
int raiseclaw(){
	mrp(0,275,260L);}	
int lowerclaw(){
	mrp(0,275,-260L);}
	
	
int crossthebufferandgettothewall(){
/*	create_drive_straight(100);
	while(analog10(5)>200){
			sleep(.05);
	}
	create_stop(); 
	Drive x cm
	Turning function here
	*/
	
	while(!gc_rbump && !gc_lbump){
		create_drive_straight(200);
		create_sensor_update();
		}
	create_stop();
}

int grabthepoms(){
	
	while(!gc_rbump && !gc_lbump){
		create_drive(120,275);
		create_sensor_update();
		}	
	create_stop();	
	create_spin_CCW(60);
	sleep(1.0);
	
	lowerclaw();
	openclaw();
	
	while(!digital(TOUCHSENSORPORT)){
		create_drive_straight(-100);
		}
	create_stop();
	closeclaw();
	sleep(1.5);
	create_drive_straight(-200);
	sleep(2.0);
		
}

int returntobase(){
	
	create_spin_CCW(60);
	sleep(1.0);
	create_stop();
	
	create_drive_straight(-200);
	sleep(4.0);
	create_stop();
	
	create_spin_CCW(60);
	sleep(1.0);
	create_stop();
	
	create_drive_straight(-200);
	sleep(3.0);
	create_stop();

	openclaw();	
	raiseclaw();
	closeclaw();	

}

int sweep(){
	set_servo_position(1,0);
	sleep(2.0);
	create_drive(-60, 275);
	sleep(8.0);
	set_servo_position(2,1900);
	create_stop();
}



int main()
{
create_connect();
create_safe();
create_sensor_update();
	
	crossthebufferandgettothewall();
	sweep();
	grabthepoms();
	returntobase();
	
create_stop();        
create_disconnect();
}
